#include <string.h>
#include <nuttx/config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/prctl.h>
#include <nuttx/pthread.h>
#include <nuttx/sched.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "Runnable.h"
#include "Thread.h"


Thread::Thread(const char *name, int stackSize, int priority, Runnable *runnable)
	: runnable_(runnable)
{
	pthread_attr_t attr;
	pthread_attr_init(&attr);

	sched_param param;
	param.sched_priority = priority;
	(void)pthread_attr_setschedparam(&attr, &param);

	pthread_attr_setstacksize(&attr, stackSize);

	if (0 != pthread_create(&pthread_, &attr, Thread::execute, this))
		errx(1, "error creating thread");
}

Thread::~Thread() {
	runnable_->setExitFlag();
	while (runnable_->isRunning())
		sleep(50);
	delete runnable_;
}

void Thread::join() {
	pthread_join(pthread_, (void **)0);
}

void Thread::sleep(uint32_t msec) {
	usleep(msec * 1000);
}

void Thread::yield() {
	usleep(1000);
}

uint32_t Thread::getTime() {
	return hrt_absolute_time() / 1000;
}

void * Thread::execute(void *p) {
	static_cast<Thread *>(p)->runnable_->run_();
	return (void *)0;
}


//////////////////////////////////////////////////////////////////////////////////////////


Period::Period(uint32_t period) {
	set(period);
}

void Period::set(uint32_t period) {
	period_ = period;
	lastWakeTime_ = Thread::getTime();
}

void Period::wait() {
	int32_t delta = (int32_t)period_ - (int32_t)(Thread::getTime() - lastWakeTime_);
	
	if (delta > 0 && delta <= (int32_t)period_)
		Thread::sleep(delta);

	lastWakeTime_ = Thread::getTime();
}

